#include <rclcpp/rclcpp.hpp>
#include <opentelemetry/trace/provider.h>
#include <opentelemetry/exporters/ostream/span_exporter.h>
#include <opentelemetry/sdk/trace/simple_processor.h>
#include <opentelemetry/sdk/trace/tracer_provider.h>
#include <opentelemetry/sdk/trace/samplers/always_on.h>
#include <opentelemetry/sdk/resource/resource.h>
#include <opentelemetry/nostd/shared_ptr.h>
#include <opentelemetry/nostd/unique_ptr.h>

#include <chrono>
#include <thread>
#include <string>
#include <vector>
#include <memory>
#include <sys/resource.h>
#include <unistd.h>

namespace trace_api = opentelemetry::trace;
namespace trace_sdk = opentelemetry::sdk::trace;
namespace resource_sdk = opentelemetry::sdk::resource;
namespace nostd = opentelemetry::nostd;

// Resource monitoring utilities
struct ResourceStats {
    long cpu_time_user;
    long cpu_time_system;
    long max_memory_kb;
    long page_faults_major;
    long page_faults_minor;
    long context_switches;
    long voluntary_switches;
    long involuntary_switches;
    
    void reset() {
        cpu_time_user = 0;
        cpu_time_system = 0;
        max_memory_kb = 0;
        page_faults_major = 0;
        page_faults_minor = 0;
        context_switches = 0;
        voluntary_switches = 0;
        involuntary_switches = 0;
    }
};

class ResourceMonitor {
public:
    static ResourceStats getCurrentStats() {
        ResourceStats stats;
        struct rusage usage;
        
        if (getrusage(RUSAGE_SELF, &usage) == 0) {
            stats.cpu_time_user = usage.ru_utime.tv_sec * 1000000 + usage.ru_utime.tv_usec;
            stats.cpu_time_system = usage.ru_stime.tv_sec * 1000000 + usage.ru_stime.tv_usec;
            stats.max_memory_kb = usage.ru_maxrss;
            stats.page_faults_major = usage.ru_majflt;
            stats.page_faults_minor = usage.ru_minflt;
            stats.context_switches = usage.ru_nvcsw + usage.ru_nivcsw;
            stats.voluntary_switches = usage.ru_nvcsw;
            stats.involuntary_switches = usage.ru_nivcsw;
        }
        
        return stats;
    }
    
    static ResourceStats getDifference(const ResourceStats& start, const ResourceStats& end) {
        ResourceStats diff;
        diff.cpu_time_user = end.cpu_time_user - start.cpu_time_user;
        diff.cpu_time_system = end.cpu_time_system - start.cpu_time_system;
        diff.max_memory_kb = end.max_memory_kb - start.max_memory_kb;
        diff.page_faults_major = end.page_faults_major - start.page_faults_major;
        diff.page_faults_minor = end.page_faults_minor - start.page_faults_minor;
        diff.context_switches = end.context_switches - start.context_switches;
        diff.voluntary_switches = end.voluntary_switches - start.voluntary_switches;
        diff.involuntary_switches = end.involuntary_switches - start.involuntary_switches;
        return diff;
    }
};

// Initialize OpenTelemetry tracing
void InitTracer() {
    auto exporter = std::unique_ptr<trace_sdk::SpanExporter>(
        new opentelemetry::exporter::trace::OStreamSpanExporter);
    
    auto processor = std::unique_ptr<trace_sdk::SpanProcessor>(
        new trace_sdk::SimpleSpanProcessor(std::move(exporter)));
    
    auto resource = resource_sdk::Resource::Create({
        {"service.name", "ros2-resource-monitor"},
        {"service.version", "1.0.0"},
        {"service.instance.id", "resource-monitor-001"}
    });
    
    auto sampler = std::unique_ptr<trace_sdk::Sampler>(
        new trace_sdk::AlwaysOnSampler);
    
    auto provider = nostd::shared_ptr<trace_api::TracerProvider>(
        new trace_sdk::TracerProvider(std::move(processor), resource, std::move(sampler)));
    
    trace_api::Provider::SetTracerProvider(provider);
}

class ResourceTestNode : public rclcpp::Node {
public:
    ResourceTestNode() : Node("resource_test_node") {
        // Initialize OpenTelemetry
        InitTracer();
        tracer_ = trace_api::Provider::GetTracerProvider()->GetTracer("resource-test-node");
        
        RCLCPP_INFO(this->get_logger(), "Resource Test Node started");
        RCLCPP_INFO(this->get_logger(), "Testing OpenTelemetry resource consumption...");
        
        // Run different tests
        runBasicTracingTest();
        runHighFrequencyTest();
        runLargeAttributeTest();
        runConcurrentSpanTest();
        runDeepNestingTest();
        runMemoryStressTest();
    }

private:
    void runBasicTracingTest() {
        RCLCPP_INFO(this->get_logger(), "\n=== Basic Tracing Test ===");
        
        auto start_stats = ResourceMonitor::getCurrentStats();
        
        for (int i = 0; i < 100; ++i) {
            auto span = tracer_->StartSpan("basic_test_span", {
                {"test.type", "basic"},
                {"iteration", static_cast<int64_t>(i)}
            });
            
            auto scope = tracer_->WithActiveSpan(span);
            
            // Simulate some work
            std::this_thread::sleep_for(std::chrono::microseconds(100));
            
            span->SetAttribute("result", "success");
            span->AddEvent("work_completed");
            span->SetStatus(trace_api::StatusCode::kOk);
            span->End();
        }
        
        auto end_stats = ResourceMonitor::getCurrentStats();
        auto diff = ResourceMonitor::getDifference(start_stats, end_stats);
        
        RCLCPP_INFO(this->get_logger(), "Basic Test Results (100 spans):");
        RCLCPP_INFO(this->get_logger(), "  CPU Time (User): %ld μs", diff.cpu_time_user);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (System): %ld μs", diff.cpu_time_system);
        RCLCPP_INFO(this->get_logger(), "  Memory Usage: %ld KB", diff.max_memory_kb);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Major): %ld", diff.page_faults_major);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Minor): %ld", diff.page_faults_minor);
        RCLCPP_INFO(this->get_logger(), "  Context Switches: %ld", diff.context_switches);
    }
    
    void runHighFrequencyTest() {
        RCLCPP_INFO(this->get_logger(), "\n=== High Frequency Test ===");
        
        auto start_stats = ResourceMonitor::getCurrentStats();
        
        for (int i = 0; i < 1000; ++i) {
            auto span = tracer_->StartSpan("high_freq_span", {
                {"test.type", "high_frequency"},
                {"iteration", static_cast<int64_t>(i)}
            });
            
            auto scope = tracer_->WithActiveSpan(span);
            
            // Minimal work
            span->SetStatus(trace_api::StatusCode::kOk);
            span->End();
        }
        
        auto end_stats = ResourceMonitor::getCurrentStats();
        auto diff = ResourceMonitor::getDifference(start_stats, end_stats);
        
        RCLCPP_INFO(this->get_logger(), "High Frequency Test Results (1000 spans):");
        RCLCPP_INFO(this->get_logger(), "  CPU Time (User): %ld μs", diff.cpu_time_user);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (System): %ld μs", diff.cpu_time_system);
        RCLCPP_INFO(this->get_logger(), "  Memory Usage: %ld KB", diff.max_memory_kb);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Major): %ld", diff.page_faults_major);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Minor): %ld", diff.page_faults_minor);
        RCLCPP_INFO(this->get_logger(), "  Context Switches: %ld", diff.context_switches);
    }
    
    void runLargeAttributeTest() {
        RCLCPP_INFO(this->get_logger(), "\n=== Large Attribute Test ===");
        
        auto start_stats = ResourceMonitor::getCurrentStats();
        
        for (int i = 0; i < 50; ++i) {
            auto span = tracer_->StartSpan("large_attr_span", {
                {"test.type", "large_attributes"},
                {"iteration", static_cast<int64_t>(i)}
            });
            
            auto scope = tracer_->WithActiveSpan(span);
            
            // Add many attributes
            for (int j = 0; j < 100; ++j) {
                span->SetAttribute("attr_" + std::to_string(j), "value_" + std::to_string(j));
            }
            
            // Add many events
            for (int j = 0; j < 50; ++j) {
                span->AddEvent("event_" + std::to_string(j));
            }
            
            span->SetStatus(trace_api::StatusCode::kOk);
            span->End();
        }
        
        auto end_stats = ResourceMonitor::getCurrentStats();
        auto diff = ResourceMonitor::getDifference(start_stats, end_stats);
        
        RCLCPP_INFO(this->get_logger(), "Large Attribute Test Results (50 spans, 100 attrs + 50 events each):");
        RCLCPP_INFO(this->get_logger(), "  CPU Time (User): %ld μs", diff.cpu_time_user);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (System): %ld μs", diff.cpu_time_system);
        RCLCPP_INFO(this->get_logger(), "  Memory Usage: %ld KB", diff.max_memory_kb);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Major): %ld", diff.page_faults_major);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Minor): %ld", diff.page_faults_minor);
        RCLCPP_INFO(this->get_logger(), "  Context Switches: %ld", diff.context_switches);
    }
    
    void runConcurrentSpanTest() {
        RCLCPP_INFO(this->get_logger(), "\n=== Concurrent Span Test ===");
        
        auto start_stats = ResourceMonitor::getCurrentStats();
        
        std::vector<std::thread> threads;
        const int num_threads = 10;
        const int spans_per_thread = 100;
        
        for (int t = 0; t < num_threads; ++t) {
            threads.emplace_back([this, t, spans_per_thread]() {
                for (int i = 0; i < spans_per_thread; ++i) {
                    auto span = tracer_->StartSpan("concurrent_span", {
                        {"test.type", "concurrent"},
                        {"thread_id", static_cast<int64_t>(t)},
                        {"iteration", static_cast<int64_t>(i)}
                    });
                    
                    auto scope = tracer_->WithActiveSpan(span);
                    
                    // Simulate some work
                    std::this_thread::sleep_for(std::chrono::microseconds(50));
                    
                    span->SetStatus(trace_api::StatusCode::kOk);
                    span->End();
                }
            });
        }
        
        // Wait for all threads to complete
        for (auto& thread : threads) {
            thread.join();
        }
        
        auto end_stats = ResourceMonitor::getCurrentStats();
        auto diff = ResourceMonitor::getDifference(start_stats, end_stats);
        
        RCLCPP_INFO(this->get_logger(), "Concurrent Test Results (%d threads, %d spans each):", num_threads, spans_per_thread);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (User): %ld μs", diff.cpu_time_user);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (System): %ld μs", diff.cpu_time_system);
        RCLCPP_INFO(this->get_logger(), "  Memory Usage: %ld KB", diff.max_memory_kb);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Major): %ld", diff.page_faults_major);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Minor): %ld", diff.page_faults_minor);
        RCLCPP_INFO(this->get_logger(), "  Context Switches: %ld", diff.context_switches);
    }
    
    void runDeepNestingTest() {
        RCLCPP_INFO(this->get_logger(), "\n=== Deep Nesting Test ===");
        
        auto start_stats = ResourceMonitor::getCurrentStats();
        
        const int nesting_depth = 20;
        
        for (int i = 0; i < 10; ++i) {
            createNestedSpans(0, nesting_depth, i);
        }
        
        auto end_stats = ResourceMonitor::getCurrentStats();
        auto diff = ResourceMonitor::getDifference(start_stats, end_stats);
        
        RCLCPP_INFO(this->get_logger(), "Deep Nesting Test Results (10 traces, depth %d):", nesting_depth);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (User): %ld μs", diff.cpu_time_user);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (System): %ld μs", diff.cpu_time_system);
        RCLCPP_INFO(this->get_logger(), "  Memory Usage: %ld KB", diff.max_memory_kb);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Major): %ld", diff.page_faults_major);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Minor): %ld", diff.page_faults_minor);
        RCLCPP_INFO(this->get_logger(), "  Context Switches: %ld", diff.context_switches);
    }
    
    void createNestedSpans(int current_depth, int max_depth, int trace_id) {
        if (current_depth >= max_depth) {
            return;
        }
        
        auto span = tracer_->StartSpan("nested_span_" + std::to_string(current_depth), {
            {"test.type", "deep_nesting"},
            {"trace_id", static_cast<int64_t>(trace_id)},
            {"depth", static_cast<int64_t>(current_depth)}
        });
        
        auto scope = tracer_->WithActiveSpan(span);
        
        // Simulate some work
        std::this_thread::sleep_for(std::chrono::microseconds(10));
        
        // Create child span
        createNestedSpans(current_depth + 1, max_depth, trace_id);
        
        span->SetStatus(trace_api::StatusCode::kOk);
        span->End();
    }
    
    void runMemoryStressTest() {
        RCLCPP_INFO(this->get_logger(), "\n=== Memory Stress Test ===");
        
        auto start_stats = ResourceMonitor::getCurrentStats();
        
        std::vector<nostd::shared_ptr<trace_api::Span>> spans;
        const int num_spans = 10000;
        
        // Create many spans without ending them immediately
        for (int i = 0; i < num_spans; ++i) {
            auto span = tracer_->StartSpan("stress_span", {
                {"test.type", "memory_stress"},
                {"iteration", static_cast<int64_t>(i)}
            });
            
            // Add some attributes to increase memory usage
            span->SetAttribute("large_data", std::string(1000, 'A' + (i % 26)));
            
            spans.push_back(span);
        }
        
        // End all spans
        for (auto& span : spans) {
            span->SetStatus(trace_api::StatusCode::kOk);
            span->End();
        }
        
        auto end_stats = ResourceMonitor::getCurrentStats();
        auto diff = ResourceMonitor::getDifference(start_stats, end_stats);
        
        RCLCPP_INFO(this->get_logger(), "Memory Stress Test Results (%d spans with large attributes):", num_spans);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (User): %ld μs", diff.cpu_time_user);
        RCLCPP_INFO(this->get_logger(), "  CPU Time (System): %ld μs", diff.cpu_time_system);
        RCLCPP_INFO(this->get_logger(), "  Memory Usage: %ld KB", diff.max_memory_kb);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Major): %ld", diff.page_faults_major);
        RCLCPP_INFO(this->get_logger(), "  Page Faults (Minor): %ld", diff.page_faults_minor);
        RCLCPP_INFO(this->get_logger(), "  Context Switches: %ld", diff.context_switches);
    }

    nostd::shared_ptr<trace_api::Tracer> tracer_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    
    auto node = std::make_shared<ResourceTestNode>();
    
    RCLCPP_INFO(node->get_logger(), "Starting OpenTelemetry Resource Consumption Test...");
    RCLCPP_INFO(node->get_logger(), "This test will measure CPU, memory, and system resource usage");
    
    // Keep the node alive to see all test results
    std::this_thread::sleep_for(std::chrono::seconds(2));
    
    RCLCPP_INFO(node->get_logger(), "\n=== Resource Test Complete ===");
    RCLCPP_INFO(node->get_logger(), "All tests completed. Check the results above.");
    
    rclcpp::shutdown();
    
    return 0;
}
